/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "calc_stoch_conv.h"
#include "zero_array16.h"
#include <stdio.h>
#include <assert.h>

#if 0

/* good implementation but loses intermediate bits */

void CalcStochConv(
  fxpt_16       ExcVec[],                       /* 15.0 format */
  fxpt_16       LPImpResp[],                    /* 2.13 format */
  fxpt_16       Conv[MAX_CW_VEC_LEN])           /* 15.0 format */
{
  fxpt_16 *Conv_p;
  fxpt_16 *LPImpResp_p;
  int   i, j;

  /* Initialize */
  for (i=SF_LEN,Conv_p=Conv; i>0; --i,++Conv_p) {
    *Conv_p = 0;
  }

  for (i = 0; i < SF_LEN; i++) {
    if (ExcVec[i] == 1) {
      for(j=min(SF_LEN - i, LEN_TRUNC_H),Conv_p= Conv+i,LPImpResp_p=LPImpResp; j>0 ; --j,++Conv_p,++LPImpResp_p) {
        *Conv_p += (*LPImpResp_p+(1<<12))>>13;
      }
    }
	else if (ExcVec[i] == -1) {
      for(j=min(SF_LEN - i, LEN_TRUNC_H),Conv_p= Conv+i,LPImpResp_p=LPImpResp; j>0; --j,++Conv_p,++LPImpResp_p) {
        *Conv_p -= (*LPImpResp_p+(1<<12))>>13;
      }
    }
  }
}

#else

/* should perform better in quality, same or faster in speed */

void CalcStochConv(
  fxpt_16       ExcVec[],                       /* 15.0 format */
  fxpt_16       LPImpResp[],                    /* 2.13 format */
  fxpt_16       Conv[MAX_CW_VEC_LEN])           /* 15.0 format */
{
  fxpt_16 *Conv_p;
  fxpt_32 *Conv32_p;
  fxpt_16 *LPImpResp_p;
  int   i, j;
  fxpt_32 Conv32[MAX_CW_VEC_LEN];

  /* Initialize */
  ZeroArray32( Conv32, SF_LEN );

  for (i = 0; i < SF_LEN; i++) {
    if (ExcVec[i] == 1) {
      for(j=min(SF_LEN - i, LEN_TRUNC_H),Conv32_p= Conv32+i,LPImpResp_p=LPImpResp; j>0 ; --j,++Conv32_p,++LPImpResp_p) {
        *Conv32_p += *LPImpResp_p;
      }
    }
	else if (ExcVec[i] == -1) {
      for(j=min(SF_LEN - i, LEN_TRUNC_H),Conv32_p= Conv32+i,LPImpResp_p=LPImpResp; j>0; --j,++Conv32_p,++LPImpResp_p) {
        *Conv32_p -= *LPImpResp_p;
      }
    }
  }
  for( i= SF_LEN, Conv_p= Conv, Conv32_p= Conv32; i>0; --i,++Conv_p,++Conv32_p )
	  *Conv_p= fxpt_saturate16_round( *Conv32_p, 13 );
}

#endif
